time = out.t.data;
%multibody
px = out.p.data(:,2);
py = out.p.data(:,3);
pvx = out.p.data(:,4);
pvy = out.p.data(:,5);
pvy(1:20) = 0;

qcmd1 = out.qcmd1;
qcmd2 = out.qcmd2;
qcmd3 = out.qcmd3;

q1 = out.q1;
q2 = out.q2;
q3 = out.q3;

%calculate
r_0x = out.r_0x.Data;
r_0y = out.r_0y.Data;

px_ = out.p_ex.Data - r_0x(1);
py_ = out.p_ey.Data - r_0y(1);

pvx_ = out.v_ex.Data;
pvy_ = out.v_ey.Data;
pvy_(1:20) = 0;

figure(1)
fontsize = 20;
linewidth = 2;
subplot(4,1,1)
plot(time,px,'b--',time,px_,'r','linewidth',linewidth)
legend('data from multibody','data from matlab')
xlabel('time(s)')
ylabel('x position(m)')
title('End-effector position(x)')
set(gca,'fontsize',fontsize,'fontname','Times');

subplot(4,1,2)
plot(time,py,'b--',time,py_,'r','linewidth',linewidth)
legend('data from multibody','data from matlab')
xlabel('time(s)')
ylabel('y position(m)')
title('End-effector position(y)')
set(gca,'fontsize',fontsize,'fontname','Times');

subplot(4,1,3)
plot(time,pvx,'b--',time,pvx_,'r','linewidth',linewidth)
legend('data from multibody','data from matlab')
xlabel('time(s)')
ylabel('x velocity(m/s)')
title('End-effector velocity(x)')
set(gca,'fontsize',fontsize,'fontname','Times');

subplot(4,1,4)
plot(time,pvy,'b--',time,pvy_,'r','linewidth',linewidth)
legend('data from multibody','data from matlab')
xlabel('time(s)')
ylabel('y velocity(m/s)')
title('End-effector velocity(y)')
set(gca,'fontsize',fontsize,'fontname','Times');

figure(2)
subplot(4,1,1)
plot(time,(px-px_),'linewidth',linewidth)
xlabel('time(s)')
ylabel('x position(m)')
title('Error(x)')
set(gca,'fontsize',fontsize,'fontname','Times');

subplot(4,1,2)
plot(time,(py-py_),'linewidth',linewidth)
xlabel('time(s)')
ylabel('y position(m)')
title('Error(y)')
set(gca,'fontsize',fontsize,'fontname','Times');

subplot(4,1,3)
plot(time,(pvx-pvx_),'linewidth',linewidth)
xlabel('time(s)')
ylabel('x velocity(m/s)')
title('Error(v_x)')
set(gca,'fontsize',fontsize,'fontname','Times');

subplot(4,1,4)
plot(time,(pvy-pvy_),'linewidth',linewidth)
xlabel('time(s)')
ylabel('y velocity(m/s)')
title('Error(v_y)')
set(gca,'fontsize',fontsize,'fontname','Times');

figure(3);
subplot(3,1,1);
plot(time, q1,'b--',time, qcmd1, 'r','LineWidth', 1);
xlabel('time(s)')
ylabel('angle')
title('theta-1');
set(gca, 'fontsize', fontsize, 'fontname', 'Times');

subplot(3,1,2);
plot(time, q2,'b--',time, qcmd2, 'r','LineWidth', 1);
xlabel('time(s)')
ylabel('angle')
title('theta-2'); 
set(gca, 'fontsize', fontsize, 'fontname', 'Times');

subplot(3,1,3);
plot(time, q3,'b--',time, qcmd3, 'r','LineWidth', 1);
xlabel('time(s)')
ylabel('angle')
title('theta-3');
set(gca, 'fontsize', fontsize, 'fontname', 'Times');

figure(4);
subplot(3,1,1);
q1(:,1)
% plot(time,q1(:,1),'linewidth',linewidth)

plot(time, qcmd1-q1, "Color", 'g','LineWidth', 1);
xlabel('time(s)')
ylabel('error1')
title('theta-1');
set(gca, 'fontsize', fontsize, 'fontname', 'Times');

subplot(3,1,2);
plot(time, (qcmd2-q2)/10, "Color", 'r','LineWidth', 1);
xlabel('time(s)')
ylabel('error2')
title('theta-2');
set(gca, 'fontsize', fontsize, 'fontname', 'Times');

subplot(3,1,3);
plot(time, qcmd3-q3, "Color", 'b','LineWidth', 1);
xlabel('time(s)')
ylabel('error3')
title('theta-3');
set(gca, 'fontsize', fontsize, 'fontname', 'Times');
